#include "stm32f10x.h"                  // Device header
#include "CarMove.h"
#include "CarPWM.h"
#include "Delay.h"
#include "PID.h"
#include "LED.h"

#define Buzzer_On GPIO_ResetBits(GPIOB,GPIO_Pin_5)
#define Buzzer_Off GPIO_SetBits(GPIOB,GPIO_Pin_5)

int track_error;
int Track_PID_Out,Follow_PID_Out;
extern int speed;
extern unsigned int distance;
extern tPid PidTrack,PidFollow;

unsigned char stop_up,stop_down;
extern unsigned char stop_cnt,buzzer_flag,slow_flag,check_flag,hcsr04_flag,RxData;
extern int speed,buzzer_time;
extern unsigned char gray[8];

#define L4 gray[0]
#define L3 gray[1]
#define L2 gray[2]
#define L1 gray[3]
#define R1 gray[4]
#define R2 gray[5]
#define R3 gray[6]
#define R4 gray[7]


void Track_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	GPIO_InitTypeDef GPIO_Initstructure;

	GPIO_Initstructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_Initstructure.GPIO_Pin=GPIO_Pin_5;
	GPIO_Initstructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_Initstructure);
	
	GPIO_SetBits(GPIOB,GPIO_Pin_5);
}

void CarFlag_Check(void)
{                                  //||(L2==0&&L1==0&&R1==0&&R2==0)
	//if((L2==0&&L1==0&&R1==0&&R2==1)||(L2==1&&L1==0&&R1==0&&R2==0)){if(check_flag==0)stop_up=1;} 	//A发车 
	if((L3==0&&L2==0&&L1==0&&R1==0)||(L1==0&&R1==0&&R2==0&&R3==0)||(L2==0&&L1==0&&R1==0&&R2==0)||(L2==1&&L1==0&&R1==0&&R2==0))
  {if(check_flag==0)stop_up=1;} 
  else{stop_up=0;stop_down=1;}
	if(stop_up==1&&stop_down==1){stop_cnt++;buzzer_flag=1;stop_down=0;}   //到A点 蜂鸣器响
	if(buzzer_flag==1){Buzzer_On;if(buzzer_time>500)buzzer_flag=0;}
	else{buzzer_time=0;Buzzer_Off;}
}

//外圈
void CarPid_TrackOut(void)
{
	if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))  //  直走
	{
		track_error=0;              
	}
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1))    // 右转
	{
		track_error=-1;               
	}
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1))	  // 左转
	{               
		track_error=1;                             
	}	
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1))   // 右转
	{
		track_error=-1;          //36          
	}	
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==0&&R4==1))   //  右转
	{
		track_error=-1;          //57          
	}	
	else if((L4==1&&L3==0&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))	 // 直走
	{
		track_error=0;            //25                   
	}

	LED_OFF();
}

//内圈
void CarPid_TrackIn(void)
{
if((L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1))            // 直走
	{
		track_error=0;             
	}
	else if((L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1))    // 右转
	{
		track_error=-1;              
	}
	else if((L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1))	  // 左转
	{
		track_error=1;                              
	}
	else if (L3==0)
	{
		if(slow_flag==0)track_error =1;
	}

	LED_ON();
}


void Car_TrackPid(void)
{
	Track_PID_Out=PID_realize(&PidTrack,track_error);
	if(Track_PID_Out>speed)Track_PID_Out=speed;
	if(Track_PID_Out<-speed)Track_PID_Out=-speed;
	
	if(hcsr04_flag==1)
	{
		if(distance<400)
		{	
			Follow_PID_Out=PID_realize(&PidFollow,distance);
			if(Follow_PID_Out>100)Follow_PID_Out=100;
			if(Follow_PID_Out<-100)Follow_PID_Out=-100;
		}
		else{Follow_PID_Out=0;}
	}
		
  	Car_SetSpeed(speed+Track_PID_Out+Follow_PID_Out,speed-Track_PID_Out+Follow_PID_Out);
//  Car_SetSpeed(speed+Track_PID_Out,speed-Track_PID_Out);

}

